/*
 * SUDSC2Interface.h
 *
 *  Created on: June 21, 2011
 *      Author: shinobiboriken
 */

#include "SUDSC2Interface.h"

SUDSC2Interface::SUDSC2Interface() {

	//Initialize everything

	//Specific sensor status
	//0 = nothing, 1 = distant, 2 = detection
	int leftSensorState = 0;
	int rightSensorState = 0;

	//Instance of the driver interface

	//Left and Right detection trackers
	bool leftDetection = false;
	bool rightDetection = false;
}

bool* SUDSC2Interface::update() {

	bool* output;
	bool temp[2] = {false,false};
	output = temp;

	SUDSSensorMinion* MrMinion = new SUDSSensorMinion();

	MrMinion->checkSensors();
	leftSensorState = MrMinion->getLeftSensorState();
	rightSensorState = MrMinion->getRightSensorState();

	(leftSensorState == 2) ? leftDetection = true : leftDetection = false;

	(rightSensorState == 2) ? rightDetection = true : rightDetection = false;

	temp[0] = leftDetection;
	temp[1] = rightDetection;

	return output;
}

int SUDSC2Interface::pollLeftSensor() {
	return leftSensorState;
}

int SUDSC2Interface::pollRightSensor() {
	return rightSensorState;
}

bool* SUDSC2Interface::checkDetections() {

	bool* output;
	bool temp[2] = {false,false};
	output = temp;

	temp[0] = leftDetection;
	temp[1] = rightDetection;

	return output;

}

int main(void){

    time_t rightnow;
    time_t start;
    int diff = 0;

	SUDSC2Interface* SUDS = new SUDSC2Interface();

	bool temp = SUDS->update();

	printf("This is the value on the left Sensor %i", SUDS->pollLeftSensor());
	printf("This is the value on the right Sensor %i", SUDS->pollRightSensor());

    time(&start);
    for(;;){
        time(&rightnow);
        diff = difftime(rightnow,start);
        if(diff == 5)
            break;
    }

	temp = SUDS->update();

	printf("This is the value on the left Sensor %i", SUDS->pollLeftSensor());
	printf("This is the value on the right Sensor %i", SUDS->pollRightSensor());

	return 0;
}

